% 快速点云可视化工具
% 作者: Chen Qiabang
% 日期: 2025-10-14
% 用途: 快速查看特定腿或电机的点云数据

function quick_plot(csv_filename, view_mode, id)
    % 参数说明:
    %   csv_filename - CSV文件路径
    %   view_mode    - 'leg' 或 'motor'
    %   id           - 腿ID(0-3) 或 电机ID(0-3)
    %
    % 使用示例:
    %   quick_plot('data.csv', 'leg', 0)     % 查看Leg 0的4个电机
    %   quick_plot('data.csv', 'motor', 2)   % 查看Motor 3在4条腿上的表现
    %   quick_plot()                         % 交互式选择
    
    %% 参数处理
    if nargin < 1
        [filename, pathname] = uigetfile('*.csv', '选择点云数据CSV文件');
        if isequal(filename, 0)
            return;
        end
        csv_filename = fullfile(pathname, filename);
        
        % 询问视图模式
        choice = questdlg('选择查看模式:', '视图选择', ...
                         '按腿部', '按电机', '按腿部');
        if isempty(choice)
            return;
        end
        view_mode = lower(choice(3:4));  % '腿部' or '电机'
        if strcmp(view_mode, '腿部')
            view_mode = 'leg';
        else
            view_mode = 'motor';
        end
        
        % 询问ID
        if strcmp(view_mode, 'leg')
            id = inputdlg('输入腿ID (0-3):', '选择腿部', 1, {'0'});
        else
            id = inputdlg('输入电机ID (0-3):', '选择电机', 1, {'0'});
        end
        
        if isempty(id)
            return;
        end
        id = str2double(id{1});
    end
    
    %% 数据加载
    opts = detectImportOptions(csv_filename);
    opts.DataLine = 6;
    opts.VariableNamesLine = 5;
    data = readtable(csv_filename, opts);
    
    leg_ids = data.Leg_ID;
    motor_ids = data.Motor_ID;
    torques = data.Torque_Nm_;
    speeds = data.Speed_rad_s_;
    
    % 移除无效数据
    valid_idx = ~isnan(torques) & ~isnan(speeds);
    torques = torques(valid_idx);
    speeds = speeds(valid_idx);
    leg_ids = leg_ids(valid_idx);
    motor_ids = motor_ids(valid_idx);
    
    %% 可视化
    leg_names = {'Leg 0', 'Leg 1', 'Leg 2', 'Leg 3'};
    motor_names = {'Motor 1', 'Motor 2', 'Motor 3', 'Motor 4'};
    colors = [0 0.4470 0.7410; 0.8500 0.3250 0.0980; ...
              0.9290 0.6940 0.1250; 0.4940 0.1840 0.5560];
    
    figure('Position', [100, 100, 1400, 800]);
    
    if strcmp(view_mode, 'leg')
        % 按腿部视图
        sgtitle(sprintf('☁️ %s 点云分析', leg_names{id+1}), ...
                'FontSize', 16, 'FontWeight', 'bold');
        
        for motor_id = 0:3
            mask = (leg_ids == id) & (motor_ids == motor_id);
            m_torques = torques(mask);
            m_speeds = speeds(mask);
            
            if isempty(m_torques)
                continue;
            end
            
            subplot(2, 2, motor_id + 1);
            scatter(m_speeds, m_torques, 3, colors(motor_id+1,:), ...
                    'filled', 'MarkerFaceAlpha', 0.5);
            
            xlabel('速度 (rad/s)', 'FontSize', 11);
            ylabel('扭矩 (Nm)', 'FontSize', 11);
            title(sprintf('%s\n数据点: %d | 扭矩: %.2f±%.2f Nm | 速度: %.2f±%.2f rad/s', ...
                  motor_names{motor_id+1}, length(m_torques), ...
                  mean(m_torques), std(m_torques), ...
                  mean(m_speeds), std(m_speeds)), ...
                  'FontSize', 10, 'FontWeight', 'bold');
            grid on;
            axis tight;
        end
        
    else
        % 按电机视图
        sgtitle(sprintf('☁️ %s 点云分析', motor_names{id+1}), ...
                'FontSize', 16, 'FontWeight', 'bold');
        
        for leg_id = 0:3
            mask = (motor_ids == id) & (leg_ids == leg_id);
            l_torques = torques(mask);
            l_speeds = speeds(mask);
            
            if isempty(l_torques)
                continue;
            end
            
            subplot(2, 2, leg_id + 1);
            scatter(l_speeds, l_torques, 3, colors(leg_id+1,:), ...
                    'filled', 'MarkerFaceAlpha', 0.5);
            
            xlabel('速度 (rad/s)', 'FontSize', 11);
            ylabel('扭矩 (Nm)', 'FontSize', 11);
            title(sprintf('%s\n数据点: %d | 扭矩: %.2f±%.2f Nm | 速度: %.2f±%.2f rad/s', ...
                  leg_names{leg_id+1}, length(l_torques), ...
                  mean(l_torques), std(l_torques), ...
                  mean(l_speeds), std(l_speeds)), ...
                  'FontSize', 10, 'FontWeight', 'bold');
            grid on;
            axis tight;
        end
    end
    
    fprintf('✓ 可视化完成！\n');
end

% 快速使用示例:
% quick_plot()  % 交互式选择
% quick_plot('data.csv', 'leg', 0)     % 查看Leg 0
% quick_plot('data.csv', 'motor', 2)   % 查看Motor 3

